#ifndef MATCHER_H
#define MATCHER_H

#include <map>
#include <utility>
#include <functional>
#include "postprocess/common/common_func.h"
#include "postprocess/common/common_var.h"
#include "postprocess/common/object_track.h"
#include "postprocess/matcher/cost.h"
#include "postprocess/matcher/optimizer.h"

using TrackObjectPair = std::pair<std::int32_t, std::int32_t>;
// using cost_func = float (*)(ObjectTrackPtr const &track,
//                             Eigen::VectorXf const &track_predict,
//                             TrackedObjectPtr const &new_object);
using cost_func = std::function<float(ObjectTrackPtr const &track,
                                      Eigen::VectorXf const &track_predict,
                                      TrackedObjectPtr const &new_object)>;
using cost_func_withpos = std::function<float(
    ObjectTrackPtr const &track, Eigen::VectorXf const &track_predict,
    TrackedObjectPtr const &new_object, const Eigen::Matrix4f &pos)>;
// using esti_func = bool (*)(float association_value, float
// connected_threshold);
using esti_func =
    std::function<bool(float association_value, float connected_threshold)>;

class Mathcer {
 public:
  Mathcer(const ProcessorClass &flag);
  Mathcer(const ProcessorClass &flag, const PostProcessorSettings &settings);
  Mathcer(Mathcer const &) = delete;             // 不允许拷贝构造函数
  Mathcer &operator=(Mathcer const &) = delete;  // 不允许赋值运算符重载
  Mathcer(Mathcer &&) = delete;                  // 不允许左值传递
  Mathcer &operator=(Mathcer &&) = delete;  // 不允许重载地址传递的运算符
  ~Mathcer();

  /// @brief main Match function
  /// @param void
  /// @return void
  void Match(std::vector<TrackedObjectPtr> *objects,
             const std::vector<ObjectTrackPtr> &tracks,
             const std::vector<Eigen::VectorXf> &tracks_predict,
             std::vector<std::pair<int, int>> *assignments,
             std::vector<int> *unassigned_tracks,
             std::vector<int> *unassigned_objects, const Eigen::Matrix4f &pos);

  /// @brief main Match function
  /// @param void
  /// @return void
  void Match(std::vector<TrackedObjectPtr> *objects,
             const std::vector<ObjectTrackPtr> &tracks,
             const std::vector<Eigen::VectorXf> &tracks_predict,
             std::vector<std::pair<int, int>> *assignments,
             std::vector<int> *unassigned_tracks,
             std::vector<int> *unassigned_objects);

  /// @brief main Match function
  /// @param void
  /// @return void
  inline void SetProcessorClass(const ProcessorClass &flag) {
    processor_flag_ = flag;
  }

 private:
  /// @brief initialize matcher
  /// @param void
  /// @return void
  void init();

  /// @brief compute association matrix
  /// @param[IN] tracks: maintained tracks for matching
  /// @param[IN] tracks_predict: predicted states of maintained tracks
  /// @param[IN] new_object: recently detected objects
  /// @param[OUT] association_mat: matrix of association distance
  /// @return nothing
  void compute_associate_matrix(
      std::vector<ObjectTrackPtr> const &tracks,
      std::vector<Eigen::VectorXf> const &tracks_predict,
      std::vector<TrackedObjectPtr> const &new_objects,
      Eigen::MatrixXf *association_mat);

  /// @brief compute association matrix
  /// @param[IN] tracks: maintained tracks for matching
  /// @param[IN] tracks_predict: predicted states of maintained tracks
  /// @param[IN] new_object: recently detected objects
  /// @param[OUT] association_mat: matrix of association distance
  /// @note transform pos is necessary before calculating
  /// @return nothing
  void compute_associate_matrix(
      std::vector<ObjectTrackPtr> const &tracks,
      std::vector<Eigen::VectorXf> const &tracks_predict,
      std::vector<TrackedObjectPtr> const &new_objects,
      Eigen::MatrixXf *association_mat, const Eigen::Matrix4f &pos);

  /// @brief compute connection_evaluation between tracks and new detected
  /// objects for association matrix
  float compute_connection_evaluation(ObjectTrackPtr const &track,
                                      Eigen::VectorXf const &track_predict,
                                      TrackedObjectPtr const &new_object);

  /// @brief compute connection_evaluation between tracks and new detected
  /// objects for association matrix
  float compute_connection_evaluation(ObjectTrackPtr const &track,
                                      Eigen::VectorXf const &track_predict,
                                      TrackedObjectPtr const &new_object,
                                      const Eigen::Matrix4f &pos);

  /// @brief compute cos theta from xy
  float VectorCosTheta2fXy(Eigen::Vector3f const &v1,
                           Eigen::Vector3f const &v2);
  /// @brief compute connected components within given threshold
  /// @param[IN] association_mat: matrix of association distance
  /// @param[IN] connected_threshold: threshold of connected components
  /// @param[OUT] track_components: connected objects of given tracks
  /// @param[OUT] obj_components: connected tracks of given objects
  /// @return nothing
  void compute_connected_components(
      Eigen::MatrixXf const &association_mat, float const connected_threshold,
      std::vector<std::vector<std::int32_t>> *track_components,
      std::vector<std::vector<std::int32_t>> *object_components);

  /// @brief analysis connected compenent(找连通域)
  /// @param[IN] graph: 2-d vector for connected row and col
  /// @param[OUT] components: 2-d connected compenents
  /// @return nothing
  void connected_component_analysis(const std::vector<std::vector<int>> &graph,
                                    std::vector<std::vector<int>> *components);

  /// @brief match detected objects to tracks in component level
  /// @param[IN] association_mat: association matrix of all objects to tracks
  /// @param[IN] track_component: component of track idx
  /// @param[IN] object_component: component of object idx
  ///@param [IN] connection_threshold : match threshold
  /// @param[OUT] sub_assignments: component assignement pair of object & track
  /// @param[OUT] sub_unassigned_tracks: component tracks not matched
  /// @param[OUT] sub_unassigned_objects: component object not matched
  /// @return nothing
  void match_in_components(Eigen::MatrixXf const &association_mat,
                           std::vector<std::int32_t> const &track_component,
                           std::vector<std::int32_t> const &object_component,
                           float const &connection_threshold,
                           std::vector<TrackObjectPair> *sub_assignments,
                           std::vector<std::int32_t> *sub_unassigned_tracks,
                           std::vector<std::int32_t> *sub_unassigned_objects);

  /// @brief assign objects to tracks using components
  void assign_objects_to_tracks(
      Eigen::MatrixXf const &association_mat,
      const double assign_distance_maximum,
      std::vector<TrackObjectPair> *assignments,
      std::vector<std::int32_t> *unassigned_tracks,
      std::vector<std::int32_t> *unassigned_objects) noexcept;

  ///@brief estimation function
  static inline bool less_than(float association_value,
                               float connected_threshold) {
    return association_value < connected_threshold;
  }
  static inline bool greater_than(float association_value,
                                  float connected_threshold) {
    return association_value > connected_threshold;
  }

  /// @brief
  std::map<int, std::pair<cost_func_withpos, esti_func>> cost_esti_func_map_;
  /// @brief threshold for distance matching
  static float s_match_2DIOU_minimum_;
  /// @brief threshold for distance matching
  static float s_match_distance_maximum_;
  /// @brief cost instance
  Cost cost_func_;
  /// @brief proseccor flag
  ProcessorClass processor_flag_;
  /// @brief post process settings
  PostProcessorSettings settings_;
};

#endif
